
#include "pose.h"
//~ #include "solve.h"

#include <vector>
#include <cassert>
#include <iostream>

using namespace std;

/// Our choice for a radial basis function
/// @param p0 first point
/// @param p1 second point
/// @return "distance" between p0 and p1
inline double phi(const Point& p0, const Point& p1) 
{
    Vector v (p0 - p1);
    return sqrt (v*v);
}

/// Constructor for the interpolator
RbfPoseInterpolator::RbfPoseInterpolator (FrameSet& fs) 
    : frames(fs) //, matrix(0), inverseMatrix(0), indep(0)
{
    // At least one pose
    assert (frames.size()>0);

    // Collect the key centers into an array and compute a bounding box
    Bbox box (frames[0].keyPoint,frames[0].keyPoint);
    for (unsigned i = 0; i != frames.size(); i++) {
      center.push_back(frames[i].keyPoint);
      box = box+Bbox (center.back(), center.back());
    }
    
    // Establish the axes which will take part in the system (usually, all 3, but
    // may be fewer if less than 4 key poses were input).
    axes [0] = 0; 
    axes [1] = 1; 
    axes [2] = 2;
    naxes = 3;
    
    if (frames.size() < 4) {
      if (frames.size() == 1) {
          // Only one pose - trivial interpolation
          return;
      }
      // sort axes in decreasing order of dimension lengths
      if (box.len(axes[1]) > box.len(axes[0])) { 
          int tmp = axes [1]; axes [1] = axes [0]; axes[0] = tmp;
      }    
      if (box.len(axes[2]) > box.len(axes[0])) { 
          int tmp = axes [2]; axes [2] = axes [0]; axes[0] = tmp;
      }    
      if (box.len(axes[2]) > box.len(axes[1])) { 
          int tmp = axes [2]; axes [2] = axes [1]; axes[1] = tmp;
      }
      naxes = frames.size()-1;
    }
    
    // Build the matrix
    n = center.size()+naxes+1;
    //~ matrix = new double [n*n];
    
    MatrixCPPL _matrix(n,n);
    
    for (unsigned i = 0; i < center.size(); i++) {
      //~ matrix [i * n + center.size()] = 
      //~ matrix [center.size() * n + i] = 1.0;
       
      _matrix(i, center.size()) = 
      _matrix(center.size(), i) = 1.0;
       
      for (unsigned j = i; j < center.size(); j++) {
          double coef = i == j ? 0.0 : phi (center [i], center [j]);
          //~ matrix [i * n + j] = matrix [j * n + i] = coef;
          _matrix(i, j) = _matrix(j, i) = coef;
      }
      for (unsigned j = 0; j < naxes; j++) {
          //~ matrix [i * n + center.size() + 1 + j] = 
          //~ matrix [(center.size() + 1 + j) * n + i] = center [i][axes[j]];
         
         _matrix(i, center.size() + 1 + j) = 
         _matrix(center.size() + 1 + j, i) = center [i][axes[j]];
      }
    }
    
    //~ // The independent term array
    //~ indep = new double [n];
    
    // Fill remaining positions with zeroes
    for (unsigned i = center.size(); i < n; i++) {
      //~ indep [i] = 0;
      for (unsigned j = center.size(); j < n; j++) {
          //~ matrix [i*n+j]=0;
          _matrix(i,j)=0;
      }
    }
   
   // Computing the inverse of matrix
   //~ inverseMatrix = new double [n*n];
   //~ bool flag = invert(matrix, inverseMatrix, n);
    
    _inverseMatrix = CPPL::i(_matrix);
   
   // Make sure the matrix inversion worked OK.
   //~ assert(flag);
}

/// Destructor
RbfPoseInterpolator::~RbfPoseInterpolator ()
{
    //~ delete matrix;
    //~ delete inverseMatrix;
    //~ delete indep;
}

/// Returns an orthonormalized transformation as per Igarashi 2005.
/// @param t input transformation.
/// @return orthonormalized version of t.
Transformation orthoNormalize (const Transformation& t) 
{
    Vector x = normalize (Vector (t.m(0,0), t.m(1,0), t.m(2,0)));
    Vector y = normalize (Vector (t.m(0,1), t.m(1,1), t.m(2,1)));
    Vector z = normalize (Vector (t.m(0,2), t.m(1,2), t.m(2,2)));
    
    int ntries = 10;
    do {
      Vector u = normalize (Vector (cross_product (y,z)));
      Vector v = normalize (Vector (cross_product (z,x)));
      Vector w = normalize (Vector (cross_product (x,y)));
      
      x = normalize ((x + u) * 0.5);
      y = normalize ((y + v) * 0.5);
      z = normalize ((z + w) * 0.5);
    } while (ntries-- && x*y + y*z + z*x > 1.0e-4);
    
    return Transformation (x[0], y[0], z[0], t.m(0,3),
                           x[1], y[1], z[1], t.m(1,3),
                           x[2], y[2], z[2], t.m(2,3));
}
    
/// Interpolates a new pose for a given keypoint.
/// @param p position of new keypoint.
/// @param pose pose at new keypoint.
void RbfPoseInterpolator::interpolatePose (const Point& p, Pose& pose) 
{
    if (frames.size() == 1) {
      // Only one pose - trivial interpolation
      pose = frames[0].pose;
      return;
    }
    
    assert (frames.size() == center.size());
   
    CoVectorCPPL _weight(n);
    CoVectorCPPL b(n);
    b.zero();
    // The right side
    // b, lambdas
    
    // the rbf weight array
    //~ double weight [n];
    
    // Start with an empty vector
    pose.clear();
    
        // Solve the system for each object
    for (unsigned iobj = 0; iobj < frames[0].pose.size(); iobj++) {
      // Do once for each transformation coefficient
      double T[3][4];
      for (int i = 0; i < 3; i++) {
          for (int j = 0; j < 4; j++) {
            for (unsigned k = 0; k < center.size(); k++) {
                //~ indep [k] = frames [k].pose[iobj].m(i,j);
                b(k) = frames [k].pose[iobj].m(i,j);
            }
            //~ matvecmult (inverseMatrix, indep, weight, n);
            
            _weight = _inverseMatrix*b;
            
            // Compute the rbf for coeficient (i,j) of object iobj
            //~ double coef = weight [center.size()];
            double coef = _weight(center.size());
            
            // Compute the polynomial part
            for (unsigned k = 0; k < naxes; k++) {
                assert (center.size()+1+k < n);
                assert (axes [k] >= 0 && axes [k] <= 2);
                //~ coef += weight [center.size()+1+k] * p [axes[k]];
                coef += _weight(center.size()+1+k)*p[axes[k]];
            }
            
            // The summation part 
            for (unsigned k = 0; k < center.size(); k++) {
                //~ coef += weight [k] * phi (p, center[k]);
                coef += _weight(k)*phi(p, center[k]);
            }
            
            T[i][j] = coef;
          }
      }
      pose.push_back(orthoNormalize 
                (Transformation (T[0][0], T[0][1], T[0][2], T[0][3],
                                 T[1][0], T[1][1], T[1][2], T[1][3],
                                 T[2][0], T[2][1], T[2][2], T[2][3])));
    }
}

